/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
//import edu.wpi.first.wpilibj.templates.commands.BallFeederWithJoystick;
import edu.wpi.first.wpilibj.Preferences;

/**
 *
 * @author Robotics
 */
public class MotorCatapult extends Subsystem {
    
    
    private Talon PrimaryMotor;
    private Talon SecondaryMotor;
    
    private Encoder CatapultEncoder;
    
    private final double MOTOR_SPEED = 1.0; //Inverted Motors require opposite speed
    private final double STOP_SPEED = 0.0;
    
    public MotorCatapult(){
        
        PrimaryMotor = new Talon(RobotMap.PRIMARY_MOTOR_PWM);
        SecondaryMotor = new Talon(RobotMap.SECONDARY_MOTOR_PWM);
        
        CatapultEncoder = new Encoder(RobotMap.CATAPULT_ENCODER_PORTA_DIO, RobotMap.CATAPULT_ENCODER_PORTB_DIO, false, EncodingType.k2X);
        
        CatapultEncoder.start();
        
        SmartDashboard.putNumber("Catapult Duration: ", 0.055);
        SmartDashboard.putNumber("Catapult Speed: ", 1.0);
        /*Preferences.getInstance().putDouble("Catapult Duration: ", 0.25);
        Preferences.getInstance().putDouble("Catapult Speed: ", 0.25);
        Preferences.getInstance().save();*/
        
        updateEncoders();
    }
    
    public void startMotors(){
        PrimaryMotor.set(MOTOR_SPEED);
        SecondaryMotor.set(MOTOR_SPEED);
    }
    
    public void stopMotors(){
        setMotorSpeeds(STOP_SPEED);    
    }
    
    public void reverseMotors(){
        setMotorSpeeds(-MOTOR_SPEED); //-Motor_Speed is the opposite direction
    }
    
    public void setPrimMotorSpeed(double Speed){
        PrimaryMotor.set(Speed);
    }
    
    public void setSecndMotorSpeed(double Speed){
        SecondaryMotor.set(Speed);
    }
    
    public void setMotorSpeeds(double Speed){
        PrimaryMotor.set(Speed);
        SecondaryMotor.set(Speed);
    }
    
    public int getCatapultEncoderValue(){
        return CatapultEncoder.get();
    }
    
    public void resetEncoder(){
        CatapultEncoder.reset();
    }
    
    public void updateEncoders(){
        SmartDashboard.putNumber("Catapult Encoder Count: ", getCatapultEncoderValue());
        SmartDashboard.putNumber("Catapult Encoder Rate: ", CatapultEncoder.getRate());
    }
    
    public void initDefaultCommand() {
        
    // Set the default command for a subsystem here.
    //setDefaultCommand(new MySpecialCommand());
    }
}
